package com.example.ws_test;

import android.util.Log;
import org.java_websocket.client.WebSocketClient;
import org.java_websocket.handshake.ServerHandshake;
import org.json.JSONException;
import org.json.JSONObject;
import java.net.URI;
import java.net.URISyntaxException;
import java.util.ArrayList;
import java.util.List;

public class RosBridgeClient {
    private static final String TAG = "RosBridgeClient";

    private WebSocketClient webSocketClient;
    private RosBridgeListener listener;
    private boolean isConnected = false;
    private String rosBridgeUrl;
    private boolean manualDisconnect = false;

    // 订阅的话题列表
    private List<String> subscribedTopics = new ArrayList<>();

    public interface RosBridgeListener {
        void onRosConnected();
        void onRosDisconnected();
        void onRosError(String error);
        void onMessageReceived(String topic, JSONObject message);
    }

    public RosBridgeClient(RosBridgeListener listener) {
        this.listener = listener;
    }

    public void connect(String rosBridgeUrl) {
        this.rosBridgeUrl = rosBridgeUrl;
        manualDisconnect = false;

        try {
            URI serverUri = new URI(rosBridgeUrl);
            webSocketClient = new WebSocketClient(serverUri) {
                @Override
                public void onOpen(ServerHandshake handshakedata) {
                    Log.d(TAG, "Connected to ROS Bridge: " + rosBridgeUrl);
                    isConnected = true;

                    // 重新订阅之前订阅过的话题
                    for (String topic : subscribedTopics) {
                        subscribe(topic, "std_msgs/msg/String");
                    }

                    if (listener != null) {
                        listener.onRosConnected();
                    }
                }

                @Override
                public void onMessage(String message) {
                    Log.d(TAG, "Received: " + message);
                    try {
                        JSONObject jsonMessage = new JSONObject(message);

                        // 检查是否是话题消息
                        if (jsonMessage.has("op") &&
                                jsonMessage.getString("op").equals("publish") &&
                                jsonMessage.has("topic") &&
                                jsonMessage.has("msg")) {

                            String topic = jsonMessage.getString("topic");
                            JSONObject msg = jsonMessage.getJSONObject("msg");

                            if (listener != null) {
                                listener.onMessageReceived(topic, msg);
                            }
                        }
                        // 检查是否是服务响应
                        else if (jsonMessage.has("op") &&
                                jsonMessage.getString("op").equals("service_response")) {
                            Log.d(TAG, "Service response: " + jsonMessage.toString());
                        }
                    } catch (JSONException e) {
                        Log.e(TAG, "Error parsing JSON message: " + e.getMessage());
                    }
                }

                @Override
                public void onClose(int code, String reason, boolean remote) {
                    Log.d(TAG, "Connection closed: " + reason);
                    isConnected = false;

                    if (listener != null) {
                        listener.onRosDisconnected();
                    }
                }

                @Override
                public void onError(Exception ex) {
                    Log.e(TAG, "WebSocket error: " + ex.getMessage());

                    if (listener != null) {
                        listener.onRosError(ex.getMessage());
                    }
                }
            };

            // 设置连接超时
            webSocketClient.setConnectionLostTimeout(30);
            webSocketClient.connect();

        } catch (URISyntaxException e) {
            Log.e(TAG, "Invalid URI: " + e.getMessage());
            if (listener != null) {
                listener.onRosError("Invalid URI: " + e.getMessage());
            }
        }
    }

    public void disconnect() {
        manualDisconnect = true;
        if (webSocketClient != null) {
            webSocketClient.close();
        }
    }

    public void subscribe(String topic, String messageType) {
        Log.d(TAG, "0909-1 " );
        if (!isConnected) {
            Log.w(TAG, "Not connected to ROS Bridge");
            return;
        }

        try {
            JSONObject subscribeMsg = new JSONObject();
            subscribeMsg.put("op", "subscribe");
            subscribeMsg.put("topic", topic);
            subscribeMsg.put("type", messageType);

            if (webSocketClient != null) {
                webSocketClient.send(subscribeMsg.toString());
                Log.d(TAG, "Subscribed to topic: " + topic);

                // 添加到订阅列表
                if (!subscribedTopics.contains(topic)) {
                    subscribedTopics.add(topic);
                }
            }
        } catch (JSONException e) {
            Log.e(TAG, "Error creating subscribe message: " + e.getMessage());
        }
    }

    public void unsubscribe(String topic) {
        if (!isConnected) {
            Log.w(TAG, "Not connected to ROS Bridge");
            return;
        }

        try {
            JSONObject unsubscribeMsg = new JSONObject();
            unsubscribeMsg.put("op", "unsubscribe");
            unsubscribeMsg.put("topic", topic);

            if (webSocketClient != null) {
                webSocketClient.send(unsubscribeMsg.toString());
                Log.d(TAG, "Unsubscribed from topic: " + topic);

                // 从订阅列表中移除
                subscribedTopics.remove(topic);
            }
        } catch (JSONException e) {
            Log.e(TAG, "Error creating unsubscribe message: " + e.getMessage());
        }
    }

    public void publish(String topic, String messageType, JSONObject message) {
        if (!isConnected) {
            Log.w(TAG, "Not connected to ROS Bridge");
            return;
        }

        try {
            JSONObject publishMsg = new JSONObject();
            publishMsg.put("op", "publish");
            publishMsg.put("topic", topic);
            publishMsg.put("msg", message);
            publishMsg.put("type",messageType);


//            message.put("data", "I'm still online");
//            publishMsg.put("op", "publish");
//            publishMsg.put("op", "publish");
//            publishMsg.put("msg", message); // msg 必须接受一个 json 对象 ， 对象中必须有data 字段 - 2025-09-27 17:00:16 @miles_fu
//            publishMsg.put("type", "std_msgs/msg/String");

            if (webSocketClient != null) {
                webSocketClient.send(publishMsg.toString());
                Log.d(TAG, "Published to topic: " + topic);
            }
        } catch (JSONException e) {
            Log.e(TAG, "Error creating publish message: " + e.getMessage());
        }
    }

    public boolean isConnected() {
        return isConnected;
    }

    public String getRosBridgeUrl() {
        return rosBridgeUrl;
    }
}
